#include "safeio.h"
#include "config.h"
#include "rtdev/rtdev.h"
#include "rtdev/safetyboard/scbitf.h"
#include "log/zlog.hh"
#include "jkutil/iniparser.h"

using safety::BpEstop;
using safety::EStopEvent;
using safety::SafeInput;
using safety::SafeIOMgr;
using safety::SysEstop;

using namespace std;

#define SFIO_INFO(fmt, ...) zlog()->info("[SafeIO] " fmt, ##__VA_ARGS__)
#define SFIO_ERROR(fmt, ...) zlog()->error("[SafeIO] " fmt, ##__VA_ARGS__)

SafeIOMgr::SafeIOMgr() { memset(&config_, 0, sizeof(config_)); }
SafeIOMgr& SafeIOMgr::instance()
{
    static SafeIOMgr __safeiomgr;
    return __safeiomgr;
}
int SafeIOMgr::init()
{
    //确定系统
    //从scb获取安全io通道信息
    auto scb = rtdev::RtDevMgr::instance().get_scb();
    // auto sfchn = scb->safety_signal()->get_channel_info();

    //从配置文件获取安全io配置信息
    config_ = load_config(ZUC_USER_CONFIG_FILE(usersettings.ini));

    // config_.us
    return 0;
}

SafeIO_ConfigParam SafeIOMgr::load_config(std::string cfgfile)
{
    SafeIO_ConfigParam cfg;
    //从配置文件中读取安全io配置信息
    Util::IniParser ini_file(cfgfile.c_str());
    std::string section = "SAFETYFUNCTIONIO";
    ini_file.get(section + ".THREE_POSITION_ENABLING_DRAG_LIMIT", cfg.three_position_enabling_drag_limit, false);
    ini_file.get(section + ".THREE_POSITION_ENABLING_CAT1STOP", cfg.three_position_enabling_cat1stop, false);

    auto& in = cfg.usr_safein_func_channel;
    ini_file.get(section + ".IN_EXT_ESTOP", in.ext_stop, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_EXT_SFSTOP", in.ext_sfstop, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_RESET_EXT_SFSTOP", in.reset_ext_sfstop, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_REDUCED_MODE", in.reduced_mode, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_THREE_POSITION_ENABLING", in.three_position_enabling, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_COLLISION_CLOSE", in.collision_close, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_COLLISION_LV1", in.collision_lv1, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_COLLISION_LV2", in.collision_lv2, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_COLLISION_LV3", in.collision_lv3, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_COLLISION_LV4", in.collision_lv4, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_COLLISION_LV5", in.collision_lv5, -1, -1, SAFEIO_USRINPUT_MAX_CNT);
    ini_file.get(section + ".IN_COLLISION_LV6", in.collision_lv6, -1, -1, SAFEIO_USRINPUT_MAX_CNT);

    auto& out = cfg.usr_safeout_func_channel;
    ini_file.get(section + ".OUT_SYS_ESTOP", out.sys_estop, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_SYS_SFSTOP", out.sys_sfstop, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_MOVING", out.moving, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_NOSTOP", out.nostop, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_REDUCED", out.reduced, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_NOREDUCED", out.noreduced, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_COLLISION_CLOSE", out.collision_close, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_COLLISION_LV1", out.collision_lv1, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_COLLISION_LV2", out.collision_lv2, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_COLLISION_LV3", out.collision_lv3, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_COLLISION_LV4", out.collision_lv4, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_COLLISION_LV5", out.collision_lv5, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);
    ini_file.get(section + ".OUT_COLLISION_LV6", out.collision_lv6, -1, -1, SAFEIO_USROUTPUT_MAX_CNT);

    return cfg;
}

int SafeIOMgr::save_config(std::string cfgfile, SafeIO_ConfigParam* cfg)
{
    Util::IniParser ini_file(cfgfile.c_str());
    std::string section = "SAFETYFUNCTIONIO";
    ini_file.set(section + ".THREE_POSITION_ENABLING_DRAG_LIMIT", cfg->three_position_enabling_drag_limit);
    ini_file.set(section + ".THREE_POSITION_ENABLING_CAT1STOP", cfg->three_position_enabling_cat1stop);
    auto& in = cfg->usr_safein_func_channel;
    ini_file.set(section + ".IN_EXT_ESTOP", in.ext_stop);
    ini_file.set(section + ".IN_EXT_SFSTOP", in.ext_sfstop);
    ini_file.set(section + ".IN_RESET_EXT_SFSTOP", in.reset_ext_sfstop);
    ini_file.set(section + ".IN_REDUCED_MODE", in.reduced_mode);
    ini_file.set(section + ".IN_THREE_POSITION_ENABLING", in.three_position_enabling);
    ini_file.set(section + ".IN_COLLISION_CLOSE", in.collision_close);
    ini_file.set(section + ".IN_COLLISION_LV1", in.collision_lv1);
    ini_file.set(section + ".IN_COLLISION_LV2", in.collision_lv2);
    ini_file.set(section + ".IN_COLLISION_LV3", in.collision_lv3);
    ini_file.set(section + ".IN_COLLISION_LV4", in.collision_lv4);
    ini_file.set(section + ".IN_COLLISION_LV5", in.collision_lv5);
    ini_file.set(section + ".IN_COLLISION_LV6", in.collision_lv6);

    auto& out = cfg->usr_safeout_func_channel;
    ini_file.set(section + ".OUT_SYS_ESTOP", out.sys_estop);
    ini_file.set(section + ".OUT_SYS_SFSTOP", out.sys_sfstop);
    ini_file.set(section + ".OUT_MOVING", out.moving);
    ini_file.set(section + ".OUT_NOSTOP", out.nostop);
    ini_file.set(section + ".OUT_REDUCED", out.reduced);
    ini_file.set(section + ".OUT_NOREDUCED", out.noreduced);
    ini_file.set(section + ".OUT_COLLISION_CLOSE", out.collision_close);
    ini_file.set(section + ".OUT_COLLISION_LV1", out.collision_lv1);
    ini_file.set(section + ".OUT_COLLISION_LV2", out.collision_lv2);
    ini_file.set(section + ".OUT_COLLISION_LV3", out.collision_lv3);
    ini_file.set(section + ".OUT_COLLISION_LV4", out.collision_lv4);
    ini_file.set(section + ".OUT_COLLISION_LV5", out.collision_lv5);
    ini_file.set(section + ".OUT_COLLISION_LV6", out.collision_lv6);

    return 0;
}
int SafeIOMgr::get_user_safein(std::vector<SafeIO_UsrSafeInputEv>& safein)
{
    safein.resize(config_.usr_safein_ev_cnt);

    if (config_.usr_safein_func_channel.ext_stop >= 0)
    {
        safein[config_.usr_safein_func_channel.ext_stop] = SafeIO_UsrSafeInputEv::EXT_ESTOP;
    }
    if (config_.usr_safein_func_channel.ext_sfstop >= 0)
    {
        safein[config_.usr_safein_func_channel.ext_sfstop] = SafeIO_UsrSafeInputEv::EXT_SFSTOP;
    }
    if (config_.usr_safein_func_channel.reset_ext_sfstop >= 0)
    {
        safein[config_.usr_safein_func_channel.reset_ext_sfstop] = SafeIO_UsrSafeInputEv::RESET_EXT_SFSTOP;
    }
    if (config_.usr_safein_func_channel.reduced_mode >= 0)
    {
        safein[config_.usr_safein_func_channel.reduced_mode] = SafeIO_UsrSafeInputEv::REDUCED_MODE;
    }
    if (config_.usr_safein_func_channel.three_position_enabling >= 0)
    {
        safein[config_.usr_safein_func_channel.three_position_enabling] = SafeIO_UsrSafeInputEv::THREE_POSITION_ENABLING;
    }
    if (config_.usr_safein_func_channel.collision_close >= 0)
    {
        safein[config_.usr_safein_func_channel.collision_close] = SafeIO_UsrSafeInputEv::COLLISION_CLOSE;
    }
    if (config_.usr_safein_func_channel.collision_lv1 >= 0)
    {
        safein[config_.usr_safein_func_channel.collision_lv1] = SafeIO_UsrSafeInputEv::COLLISION_LV1;
    }
    if (config_.usr_safein_func_channel.collision_lv2 >= 0)
    {
        safein[config_.usr_safein_func_channel.collision_lv2] = SafeIO_UsrSafeInputEv::COLLISION_LV2;
    }
    if (config_.usr_safein_func_channel.collision_lv3 >= 0)
    {
        safein[config_.usr_safein_func_channel.collision_lv3] = SafeIO_UsrSafeInputEv::COLLISION_LV3;
    }
    if (config_.usr_safein_func_channel.collision_lv4 >= 0)
    {
        safein[config_.usr_safein_func_channel.collision_lv4] = SafeIO_UsrSafeInputEv::COLLISION_LV4;
    }
    if (config_.usr_safein_func_channel.collision_lv5 >= 0)
    {
        safein[config_.usr_safein_func_channel.collision_lv5] = SafeIO_UsrSafeInputEv::COLLISION_LV5;
    }
    if (config_.usr_safein_func_channel.collision_lv6 >= 0)
    {
        safein[config_.usr_safein_func_channel.collision_lv6] = SafeIO_UsrSafeInputEv::COLLISION_LV6;
    }

    return 0;
}
int SafeIOMgr::set_user_safein(const std::vector<SafeIO_UsrSafeInputEv>& safein)
{
    for (size_t i = 0; i < safein.size(); i++)
    {
        switch (safein[i])
        {
        case SafeIO_UsrSafeInputEv::EXT_ESTOP:
            config_.usr_safein_func_channel.ext_stop = i;
            break;
        case SafeIO_UsrSafeInputEv::EXT_SFSTOP:
            config_.usr_safein_func_channel.ext_sfstop = i;
            break;
        case SafeIO_UsrSafeInputEv::RESET_EXT_SFSTOP:
            config_.usr_safein_func_channel.reset_ext_sfstop = i;
            break;
        case SafeIO_UsrSafeInputEv::REDUCED_MODE:
            config_.usr_safein_func_channel.reduced_mode = i;
            break;
        case SafeIO_UsrSafeInputEv::THREE_POSITION_ENABLING:
            config_.usr_safein_func_channel.three_position_enabling = i;
            break;
        case SafeIO_UsrSafeInputEv::COLLISION_CLOSE:
            config_.usr_safein_func_channel.collision_close = i;
            break;
        case SafeIO_UsrSafeInputEv::COLLISION_LV1:
            config_.usr_safein_func_channel.collision_lv1 = i;
            break;
        case SafeIO_UsrSafeInputEv::COLLISION_LV2:
            config_.usr_safein_func_channel.collision_lv2 = i;
            break;
        case SafeIO_UsrSafeInputEv::COLLISION_LV3:
            config_.usr_safein_func_channel.collision_lv3 = i;
            break;
        case SafeIO_UsrSafeInputEv::COLLISION_LV4:
            config_.usr_safein_func_channel.collision_lv4 = i;
            break;
        case SafeIO_UsrSafeInputEv::COLLISION_LV5:
            config_.usr_safein_func_channel.collision_lv5 = i;
            break;
        case SafeIO_UsrSafeInputEv::COLLISION_LV6:
            config_.usr_safein_func_channel.collision_lv6 = i;
            break;
        default:
            break;
        };
    }
    return 0;
}
int SafeIOMgr::get_user_safeout(std::vector<SafeIO_UsrSafeOutputEv>& safeout)
{
    safeout.resize(config_.usr_safeout_ev_cnt);
    if (config_.usr_safeout_func_channel.sys_estop >= 0)
    {
        safeout[config_.usr_safeout_func_channel.sys_estop] = SafeIO_UsrSafeOutputEv::SYS_ESTOP;
    }
    if (config_.usr_safeout_func_channel.sys_sfstop >= 0)
    {
        safeout[config_.usr_safeout_func_channel.sys_sfstop] = SafeIO_UsrSafeOutputEv::SYS_SFSTOP;
    }
    if (config_.usr_safeout_func_channel.moving >= 0)
    {
        safeout[config_.usr_safeout_func_channel.moving] = SafeIO_UsrSafeOutputEv::MOVING;
    }
    if (config_.usr_safeout_func_channel.nostop >= 0)
    {
        safeout[config_.usr_safeout_func_channel.nostop] = SafeIO_UsrSafeOutputEv::NOSTOP;
    }
    if (config_.usr_safeout_func_channel.reduced >= 0)
    {
        safeout[config_.usr_safeout_func_channel.reduced] = SafeIO_UsrSafeOutputEv::REDUCED;
    }
    if (config_.usr_safeout_func_channel.noreduced >= 0)
    {
        safeout[config_.usr_safeout_func_channel.noreduced] = SafeIO_UsrSafeOutputEv::NOREDUCED;
    }
    if (config_.usr_safeout_func_channel.collision_close >= 0)
    {
        safeout[config_.usr_safeout_func_channel.collision_close] = SafeIO_UsrSafeOutputEv::COLLISION_CLOSE;
    }
    if (config_.usr_safeout_func_channel.collision_lv1 >= 0)
    {
        safeout[config_.usr_safeout_func_channel.collision_lv1] = SafeIO_UsrSafeOutputEv::COLLISION_LV1;
    }
    if (config_.usr_safeout_func_channel.collision_lv2 >= 0)
    {
        safeout[config_.usr_safeout_func_channel.collision_lv2] = SafeIO_UsrSafeOutputEv::COLLISION_LV2;
    }
    if (config_.usr_safeout_func_channel.collision_lv3 >= 0)
    {
        safeout[config_.usr_safeout_func_channel.collision_lv3] = SafeIO_UsrSafeOutputEv::COLLISION_LV3;
    }
    if (config_.usr_safeout_func_channel.collision_lv4 >= 0)
    {
        safeout[config_.usr_safeout_func_channel.collision_lv4] = SafeIO_UsrSafeOutputEv::COLLISION_LV4;
    }
    if (config_.usr_safeout_func_channel.collision_lv5 >= 0)
    {
        safeout[config_.usr_safeout_func_channel.collision_lv5] = SafeIO_UsrSafeOutputEv::COLLISION_LV5;
    }
    if (config_.usr_safeout_func_channel.collision_lv6 >= 0)
    {
        safeout[config_.usr_safeout_func_channel.collision_lv6] = SafeIO_UsrSafeOutputEv::COLLISION_LV6;
    }

    return 0;
}
int SafeIOMgr::set_user_safeout(const std::vector<SafeIO_UsrSafeOutputEv>& safeout)
{
    for (size_t i = 0; i < safeout.size(); i++)
    {
        switch (safeout[i])
        {
        case SafeIO_UsrSafeOutputEv::SYS_ESTOP:
            config_.usr_safeout_func_channel.sys_estop = i;
            break;
        case SafeIO_UsrSafeOutputEv::SYS_SFSTOP:
            config_.usr_safeout_func_channel.sys_sfstop = i;
            break;
        case SafeIO_UsrSafeOutputEv::MOVING:
            config_.usr_safeout_func_channel.moving = i;
            break;
        case SafeIO_UsrSafeOutputEv::NOSTOP:
            config_.usr_safeout_func_channel.nostop = i;
            break;
        case SafeIO_UsrSafeOutputEv::REDUCED:
            config_.usr_safeout_func_channel.reduced = i;
            break;
        case SafeIO_UsrSafeOutputEv::NOREDUCED:
            config_.usr_safeout_func_channel.noreduced = i;
            break;
        case SafeIO_UsrSafeOutputEv::COLLISION_CLOSE:
            config_.usr_safeout_func_channel.collision_close = i;
            break;
        case SafeIO_UsrSafeOutputEv::COLLISION_LV1:
            config_.usr_safeout_func_channel.collision_lv1 = i;
            break;
        case SafeIO_UsrSafeOutputEv::COLLISION_LV2:
            config_.usr_safeout_func_channel.collision_lv2 = i;
            break;
        case SafeIO_UsrSafeOutputEv::COLLISION_LV3:
            config_.usr_safeout_func_channel.collision_lv3 = i;
            break;
        case SafeIO_UsrSafeOutputEv::COLLISION_LV4:
            config_.usr_safeout_func_channel.collision_lv4 = i;
            break;
        case SafeIO_UsrSafeOutputEv::COLLISION_LV5:
            config_.usr_safeout_func_channel.collision_lv5 = i;
            break;
        case SafeIO_UsrSafeOutputEv::COLLISION_LV6:
            config_.usr_safeout_func_channel.collision_lv6 = i;
            break;
        default:
            break;
        }
    }
    return 0;
}
bool SafeIOMgr::is_reduce_mode()
{
    //TODO:实现缩减模式的判断
    return false;
}
void SafeIOMgr::update() { estop.check(); }
//检查安全DI功能是否被设置
static int check_safetyDI_config(int safety_di_func)
{
    // int chnId;
    // for (chnId = 0; chnId < 8; chnId++)
    // {
    //     if (zucmotStatus->sFuncDISettings.diChnFunc[chnId] == safety_di_func)
    //     {
    //         return 1;
    //     }
    // }
    return 0;
}
void safety::check_safety_io()
{
    // TODO:移植到独立的安全IO处理模块中

    // if (zucmotConfig->cab_type != 2)
    // {
    //     zucmotStatus->safetyguard_lock = 0;
    //     //在未支持安全IO之前,保证minicab能够正常触发安全平面的保护性停止和缩减模式
    //     if (zucmotConfig->cab_type == 3)
    //     {
    //         zucmot_hal_data->safetyguard_req = safeZone_reduce_stop ? 0 : 1;
    //         (zucmot_hal_data->reduce_mode_req) = safeZone_reduce_mode ? 0 : 1;  // 0不不触发缩减   1触发缩减
    //     }
    //     return;
    // }
    // hal_s32_t temp_di_work;
    // int channel_ID;

    // SCBLocalDIType scb_local_di = get_scb_local_DI_status();
    // //default safety function protective stop  Antilogic
    // temp_di_work = 1;
    // if ((scb_local_di.local_usr_stop_1 == 0) || (scb_local_di.local_usr_stop_2 == 0))
    // {
    //     temp_di_work = 0;
    // }

    // SCBLocalDIType scb_local_di_signal = get_scb_local_DI_status();

    // //safety function DI protective stop  Antilogic
    // if (check_safetyDI_config(FUNCDI_SAFETYGUARD))
    // {
    //     temp_di_work = temp_di_work && scb_local_di_signal.local_add_sfgrd_stop;
    // }
    // if (temp_di_work == 0 && (zucmot_hal_data->safetyguard_req) == 0)
    // {
    //     reportInfo(INFO_SFDI_EX_PROTECTIVE_STOP_TRIGED, "safety function DI(additial protective stop) triggered");
    // }
    // temp_di_work = temp_di_work && safeZone_reduce_stop;  //安全平面通过此req进入保护性停止
    // zucmot_hal_data->safetyguard_req = temp_di_work ? 0 : 1;

    // //safety function DI reset protective stop
    // temp_di_work = 0;  //上升沿触发1→2,未配置功能时默认值为0
    // if (check_safetyDI_config(FUNCDI_RESET_SAFETYGUARD))
    // {
    //     if (zucmotStatus->percentage_mode_level == 3 && (zucmot_hal_data->stop_mode_req == 1 || zucmot_hal_data->safetyguard_req == 1))
    //     {
    //         zucmotStatus->safetyguard_lock = 1;
    //     }

    //     temp_di_work = scb_local_di_signal.local_sfgrd_rst ? SAFETY_DI_ON : SAFETY_DI_OFF;

    //     if (temp_di_work == SAFETY_DI_ON && (zucmot_hal_data->reset_safetyguard_req) == SAFETY_DI_OFF)
    //     {
    //         reportInfo(INFO_SFDI_PROTECTIVE_STOP_RESET_TRIGED, "safety function DI(reset protective stop) triggered");
    //     }
    // }
    // else
    // {
    //     //未开启此安全功能 以自动的形式退出保护性停止，开启此功能保护性停止退出必须靠此req
    //     //安全平面的保护性停止退出不受到此功能配置的影响
    //     zucmotStatus->safetyguard_lock = 0;
    // }
    // (zucmot_hal_data->reset_safetyguard_req) = temp_di_work;

    // //safety function DI reduce mode request handling Antilogic
    // temp_di_work = 1;
    // if (check_safetyDI_config(FUNCDI_REDUCE_MODE))
    // {
    //     temp_di_work = scb_local_di_signal.local_reduce;
    //     if (temp_di_work == 0 && (zucmot_hal_data->reduce_mode_req) == 0)
    //     {
    //         reportInfo(INFO_SFDI_REDUCED_MODE_TRIGED, "safety function DI(reduce mode) triggered");
    //     }
    // }
    // temp_di_work = temp_di_work && safeZone_reduce_mode;
    // (zucmot_hal_data->reduce_mode_req) = temp_di_work ? 0 : 1;  // 0不不触发缩减   1触发缩减

    // // safety function DI Three position enabling limit request handling Antilogic
    // temp_di_work = 0;
    // if (check_safetyDI_config(FUNCDI_THREE_POSITION_ENABLING))
    // {
    //     temp_di_work = scb_local_di_signal.local_three_position_enabling;
    //     if (temp_di_work == 0 && (zucmot_hal_data->three_position_enabling) == 0)
    //     {
    //         reportInfo(INFO_SFDI_THREE_POSITION_ENABLING, "safety function DI(three position enabling) triggered");
    //     }
    //     (zucmot_hal_data->three_position_enabling) = temp_di_work ? 0 : 1;  //退出三位置使能限制
    //     zucmotStatus->three_position_enable_limit = temp_di_work ? 0 : 1;   //1 进行限制  0 取消限制
    //     //根据 usersettings 中的安全di配置判定三位置使能功能是否限制拖拽
    //     if (zucmotStatus->sFuncDISettings.drag_limit == 1 && zucmotDebug->dragging == 1 && zucmotStatus->three_position_enable_limit == 1)
    //     {
    //         //复用功能DI退出拖拽模式req
    //         three_position_enabling_exit_drag_req = 1;
    //     }
    //     else
    //     {
    //         three_position_enabling_exit_drag_req = 0;
    //     }
    // }
    // else
    // {
    //     zucmotStatus->three_position_enable_limit = 0;
    // }

    // SCBSafetyDI scb_safety_io_signal = get_scb_saftety_DI_status();
    // //safety function DI set collision level7 mode request handling Antilogic
    // temp_di_work = 0;  //上升沿触发1→2,未配置功能时默认值为0
    // if (check_safetyDI_config(SAFETYDI_COLLISION_LEVEL7))
    // {
    //     temp_di_work = scb_safety_io_signal.safety_di_collision[6] ? SAFETY_DI_ON : SAFETY_DI_OFF;
    //     if (temp_di_work == SAFETY_DI_ON && (zucmot_hal_data->collision_lv7_req) == SAFETY_DI_OFF)
    //     {
    //         reportInfo(INFO_SFDI_SET_CLSN_LEVEL7, "safety function DI(collision level7) triggered");
    //     }
    // }
    // (zucmot_hal_data->collision_lv7_req) = temp_di_work;  // 0不触发collision level5   1触发collision level5

    // //safety function DI set collision level5 mode request handling Antilogic
    // temp_di_work = 0;  //上升沿触发1→2,未配置功能时默认值为0
    // if (check_safetyDI_config(SAFETYDI_COLLISION_LEVEL5))
    // {
    //     temp_di_work = scb_safety_io_signal.safety_di_collision[5] ? SAFETY_DI_ON : SAFETY_DI_OFF;
    //     if (temp_di_work == SAFETY_DI_ON && (zucmot_hal_data->collision_lv5_req) == SAFETY_DI_OFF)
    //     {
    //         reportInfo(INFO_SFDI_SET_CLSN_LEVEL5, "safety function DI(collision level5) triggered");
    //     }
    // }
    // (zucmot_hal_data->collision_lv5_req) = temp_di_work;  // 0不触发collision level5   1触发collision level5

    // //safety function DI set collision level4 mode request handling Antilogic
    // temp_di_work = 0;  //上升沿触发1→2,未配置功能时默认值为0
    // if (check_safetyDI_config(SAFETYDI_COLLISION_LEVEL4))
    // {
    //     temp_di_work = scb_safety_io_signal.safety_di_collision[5] ? SAFETY_DI_ON : SAFETY_DI_OFF;
    //     if (temp_di_work == SAFETY_DI_ON && (zucmot_hal_data->collision_lv4_req) == SAFETY_DI_OFF)
    //     {
    //         reportInfo(INFO_SFDI_SET_CLSN_LEVEL4, "safety function DI(collision level4) triggered");
    //     }
    // }
    // (zucmot_hal_data->collision_lv4_req) = temp_di_work;  // 0不触发collision level4   1触发collision level4

    // //safety function DI set collision level3 mode request handling Antilogic
    // temp_di_work = 0;  //上升沿触发1→2,未配置功能时默认值为0
    // if (check_safetyDI_config(SAFETYDI_COLLISION_LEVEL3))
    // {
    //     temp_di_work = scb_safety_io_signal.safety_di_collision[4] ? SAFETY_DI_ON : SAFETY_DI_OFF;
    //     if (temp_di_work == SAFETY_DI_ON && (zucmot_hal_data->collision_lv3_req) == SAFETY_DI_OFF)
    //     {
    //         reportInfo(INFO_SFDI_SET_CLSN_LEVEL3, "safety function DI(collision level3) triggered");
    //     }
    // }
    // (zucmot_hal_data->collision_lv3_req) = temp_di_work;  // 0不触发collision level3   1触发collision level3

    // //safety function DI set collision level2 mode request handling Antilogic
    // temp_di_work = 0;  //上升沿触发1→2,未配置功能时默认值为0
    // if (check_safetyDI_config(SAFETYDI_COLLISION_LEVEL2))
    // {
    //     temp_di_work = scb_safety_io_signal.safety_di_collision[3] ? SAFETY_DI_ON : SAFETY_DI_OFF;
    //     if (temp_di_work == SAFETY_DI_ON && (zucmot_hal_data->collision_lv2_req) == SAFETY_DI_OFF)
    //     {
    //         reportInfo(INFO_SFDI_SET_CLSN_LEVEL2, "safety function DI(collision level2) triggered");
    //     }
    // }
    // (zucmot_hal_data->collision_lv2_req) = temp_di_work;  // 0不触发collision level4   1触发collision level4

    // //safety function DI set collision level1 mode request handling Antilogic
    // temp_di_work = 0;  //上升沿触发1→2,未配置功能时默认值为0
    // if (check_safetyDI_config(SAFETYDI_COLLISION_LEVEL1))
    // {
    //     temp_di_work = scb_safety_io_signal.safety_di_collision[1] ? SAFETY_DI_ON : SAFETY_DI_OFF;
    //     if (temp_di_work == SAFETY_DI_ON && (zucmot_hal_data->collision_lv1_req) == SAFETY_DI_OFF)
    //     {
    //         reportInfo(INFO_SFDI_SET_CLSN_LEVEL1, "safety function DI(collision level1) triggered");
    //     }
    // }
    // (zucmot_hal_data->collision_lv1_req) = temp_di_work;  // 0不触发collision level1   1触发collision level1

    // //safety function DI set collision level0 mode request handling Antilogic
    // temp_di_work = 0;  //上升沿触发1→2,未配置功能时默认值为0
    // if (check_safetyDI_config(SAFETYDI_COLLISION_LEVEL0))
    // {
    //     temp_di_work = scb_safety_io_signal.safety_di_collision[0] ? SAFETY_DI_ON : SAFETY_DI_OFF;
    //     if (temp_di_work == SAFETY_DI_ON && (zucmot_hal_data->collision_lv0_req) == SAFETY_DI_OFF)
    //     {
    //         reportInfo(INFO_SFDI_SET_CLSN_LEVEL0, "safety function DI(collision level0) triggered");
    //     }
    // }
    // (zucmot_hal_data->collision_lv0_req) = temp_di_work;  // 0不触发collision level0  1触发collision level0

    // //safety DO  安全DO输出控制器仅做SCB暂时无法独立完成的部分，即保护性停止、缩减模式相关的输出
    // for (channel_ID = 0; channel_ID < 8; channel_ID++)
    // {
    //     // following safety DO are all antilogic
    //     switch (zucmotStatus->sFuncDOSettings.doChnFunc[channel_ID])
    //     {
    //     //系统保护性停止状态输出
    //     case FUNCDO_SAFETYGUARD: {
    //         //zucmotStatus->safetyguard_stop状态仅为实现保护性停止复位功能
    //         //目前判定控制器是否处于保护性停止状态需要使用percentage_mode_level
    //         if (zucmotStatus->percentage_mode_level == 3)
    //         {
    //             zucmotPhysicIOStruct->safety_do[channel_ID] = 0;
    //         }
    //         else
    //         {
    //             zucmotPhysicIOStruct->safety_do[channel_ID] = 1;
    //         }
    //         break;
    //     }
    //     case FUNCDO_NOTSTOPPING: {
    //         //不运动状态输出
    //         //触发急停或保护性停止，并且末端速度为0,控制器仅根据是否进入保护性停止状态发送给SCB输出请求，其余判定皆在SCB进行
    //         if (zucmotStatus->percentage_mode_level == 3)
    //         {
    //             zucmotPhysicIOStruct->safety_do[channel_ID] = 0;
    //         }
    //         else
    //         {
    //             zucmotPhysicIOStruct->safety_do[channel_ID] = 1;
    //         }
    //         break;
    //     }
    //     case FUNCDO_REDUCED_MODE: {
    //         //缩减状态输出
    //         if (zucmotStatus->reduce_mode)
    //         {
    //             zucmotPhysicIOStruct->safety_do[channel_ID] = 0;
    //         }
    //         else
    //         {
    //             zucmotPhysicIOStruct->safety_do[channel_ID] = 1;
    //         }
    //         break;
    //     }
    //     case FUNCDO_NOTREDUCEMODE: {
    //         //非缩减状态输出
    //         if (!zucmotStatus->reduce_mode)
    //         {
    //             zucmotPhysicIOStruct->safety_do[channel_ID] = 0;
    //         }
    //         else
    //         {
    //             zucmotPhysicIOStruct->safety_do[channel_ID] = 1;
    //         }
    //         break;
    //     }
    //     default: {
    //         zucmotPhysicIOStruct->safety_do[channel_ID] = 1;
    //         break;
    //     }
    //     }
    // }
}

void safety::set_safety_IO_status()
{
#if 0
    //set safet DO for SCB
    if (zucmotConfig->cab_type != 2)
    {
        return;
    }
    //SCB safety DO pairs  antilogical
    for (int i = 0; i < 8; i++) { rtdev::get_scb()->set_scb_safety_DO_cmd(zucmotPhysicIOStruct->safety_do[i], i); }
#endif
}

void safety::get_safety_IO_status()
{
#if 0
    if (zucmotConfig->cab_type != 2)
    {
        return;
    }

    SCBSafetyDI safety_di = rtdev::get_scb()->get_scb_saftety_DI_status();
    //get safety DO status from SCB  antilogical
    //暂时未用
    for (int channel_ID = 0; channel_ID < 8; channel_ID++)
        {
            switch (zucmotStatus->sFuncDOSettings.doChnFunc[channel_ID])
        {
        case FUNCDO_BTN_ESTOP: {
            zucmotStatus->synch_safety_do_status.button_estop = safety_di.feedback_sfdo_channel[channel_ID];
            break;
        }
        case FUNCDO_SYS_ESTOP: {
            zucmotStatus->synch_safety_do_status.system_estop = safety_di.feedback_sfdo_channel[channel_ID];
            break;
        }
        case FUNCDO_SAFETYGUARD: {
            zucmotStatus->synch_safety_do_status.system_safeguard = safety_di.feedback_sfdo_channel[channel_ID];
            break;
        }
        case FUNCDO_MOVING: {
            zucmotStatus->synch_safety_do_status.robot_moving = safety_di.feedback_sfdo_channel[channel_ID];
            break;
        }
        case FUNCDO_NOTSTOPPING: {
            zucmotStatus->synch_safety_do_status.not_stopping = safety_di.feedback_sfdo_channel[channel_ID];
            break;
        }
        case FUNCDO_REDUCED_MODE: {
            zucmotStatus->synch_safety_do_status.reduce_status = safety_di.feedback_sfdo_channel[channel_ID];
            break;
        }
        case FUNCDO_NOTREDUCEMODE: {
            zucmotStatus->synch_safety_do_status.not_reduce = safety_di.feedback_sfdo_channel[channel_ID];
            break;
        }

        default:
            break;
        }
    }

    SCBLocalDIType local_di = rtdev::get_scb()->get_scb_local_DI_status();
    //get safety DI status from SCB  antilogical 0x192
    zucmotStatus->synch_safety_di_status.add_estop = local_di.local_add_estop;
    zucmotStatus->synch_safety_di_status.add_sfgsafetyguard_stop = local_di.local_add_sfgrd_stop;
    zucmotStatus->synch_safety_di_status.reduce_mode = local_di.local_reduce;
    zucmotStatus->synch_safety_di_status.sfgsafetyguard_reset = local_di.local_sfgrd_rst;
    zucmotStatus->synch_safety_di_status.three_position_enabling = local_di.local_three_position_enabling;
    //get safety DI status from SCB  antilogical 0x193
#endif
}
// bool SafeEvent::is_safe()
// {
//     for (auto& input : in)
//     {
//         if (input->stat() != SafeStat::Safe)
//         {
//             return false;
//         }
//     }
//     return true;
// }
// int SafeEvent::del_input(int id)
// {
//     uint32_t cid = id;
//     for (auto itor = in.begin(); itor != in.end();)
//     {
//         if ((*itor)->id() == cid)
//         {
//             itor = in.erase(itor);
//             return 0;
//         }
//         else
//         {
//             itor++;
//         }
//     }
//     return -1;
// }
// int SafeEvent::add_input(std::shared_ptr<InputItf> input)
// {
//     for (auto itor = in.begin(); itor != in.end(); itor++)
//     {
//         if ((*itor)->id() == input->id())
//         {
//             return -1;
//         }
//     }
//     in.push_back(input);
//     return 0;
// }
// void SafeEvent::update()
// {
//     for (auto& input : in) { input->update(); }
// }

static safety::SafeStat dual_check(int chn1, int chn2, bool fault)
{
    /**
       sf0   sf1   fault        sta
        0     0      0       unsafe  ->  reset dualerr
        1     1      0       safe    ->  reset dualerr
        0     1      0       unsafe
        0     1      1       fault
        1     0      0       unsafe
        1     0      1       fault
    */
    if (fault && (chn1 != chn2))
    {
        return safety::SafeStat::Fault;
    }
    else if (chn1 && chn2)
    {
        return safety::SafeStat::Safe;
    }
    else
    {
        return safety::SafeStat::UnSafe;
    }
}

EStopEvent::EStopEvent()
{
    inputs_.push_back(std::make_shared<BpEstop>());
    inputs_.push_back(std::make_shared<SysEstop>());
    state_ = SafeStat::Safe;
}

EStopEvent::~EStopEvent() {}

int EStopEvent::check()
{
    int ret = 0;
    safety::SafeStat issafe = safety::SafeStat::Safe;
    for (auto& input : inputs_)
    {
        auto stat = input->update_new_state();
        if (stat != safety::SafeStat::Safe)
        {
            issafe = stat;
            break;
        }
    }

    if (state_ != issafe)
    {
        if (issafe == SafeStat::Safe)  //解除不安全状态
        {
            ret = 0;
            SFIO_INFO("EStopEvent is safe");
        }
        else
        {
            //进入不安全状态
            ret = -1;
            SFIO_ERROR("EStopEvent is unsafe");
        }
    }
    state_ = issafe;
    return ret;
}

BpEstop::BpEstop()
{
    signal[0] = true;
    signal[1] = true;
    fault = false;
}
safety::SafeStat BpEstop::update_new_state()
{
    auto scb = rtdev::RtDevMgr::instance().get_scb();
    if (scb.get() == nullptr || !scb->is_alive())
    {
        signal[0] = signal[1] = false;
        fault = true;
        return safety::SafeStat::Fault;
    }
    scb->safety_signal()->get_ei_stat(signal, &fault);

    auto stat = dual_check(signal[0], signal[1], fault);

    return stat;
}

SysEstop::SysEstop()
{
    signal[0] = true;
    signal[1] = true;
    fault = false;
}
safety::SafeStat SysEstop::update_new_state()
{
    auto scb = rtdev::RtDevMgr::instance().get_scb();
    if (scb.get() == nullptr || !scb->is_alive())
    {
        signal[0] = signal[1] = false;
        fault = true;
        return safety::SafeStat::Fault;
    }
    scb->safety_signal()->get_ei_stat(signal, &fault);  //FIXME:替换为SCB的急停接口

    auto stat = dual_check(signal[0], signal[1], fault);

    return stat;
}

safety::ProtectiveStopEvent::ProtectiveStopEvent()
{
    inputs_.push_back(std::make_shared<ScbPStop>());
    state_ = SafeStat::Safe;
}

int safety::ProtectiveStopEvent::check()
{
    int ret = 0;
    safety::SafeStat issafe = safety::SafeStat::Safe;
    for (auto& input : inputs_)
    {
        auto stat = input->update_new_state();
        if (stat != safety::SafeStat::Safe)
        {
            issafe = stat;
            break;
        }
    }

    if (state_ != issafe)
    {
        if (issafe == SafeStat::Safe)  //解除不安全状态?? #TODO 保护性停止的退出需要外部信号触发？
        {
            ret = 0;
            SFIO_INFO("ProtectiveStopEvent is safe");
        }
        else
        {
            //进入不安全状态
            ret = -1;
            SFIO_ERROR("ProtectiveStopEvent is unsafe");
        }
    }
    state_ = issafe;
    return ret;
}

safety::ScbPStop::ScbPStop()
{
    signal[0] = true;
    signal[1] = true;
    fault = false;
}

safety::SafeStat safety::ScbPStop::update_new_state()
{
    auto scb = rtdev::RtDevMgr::instance().get_scb();
    if (scb.get() == nullptr || !scb->is_alive())
    {
        signal[0] = signal[1] = false;
        fault = true;
        return safety::SafeStat::Fault;
    }
    scb->safety_signal()->get_si_stat(signal, &fault);
    auto stat = dual_check(signal[0], signal[1], fault);
    return stat;
}
